/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009 MobileRobots Inc.

 demoDirectMotionCommands - demo for the Simulator MobileSim
 ========================================================================

 Description:
   This demo shows the movement of the robot in the Simulator
   MobileSim. It is possible to read the position and the range of the Sonarsensors
   of the robot.
   Before you can run this demo you must open the Simulator
   MobileSim.
   
   (1) For Simulation: Comment line 48 and compile it.
       For Real robot connection: directly compile your code.

   (2) Edit your program in the proramming zone.

 References:
   Aria Reference HTML
   http://www.ai.rug.nl/vakinformatie/pas/content/Aria-manual/classes.html

 Authors:
   Steven Chiu, Louis Chen

 Copyright (c) 2010 IACSL, NTUST, Taiwan

*/

#include "Aria.h"




int main(int argc, char** argv)
{
	// Initialize some global data
	Aria::init();
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();

//*********************Simulation or Link to real Robot******************************

	char charBuffer[100];
	//"140.118.38.108": Link to P3-DX
	//"140.118.38.109": Link to Amigo
	sprintf(charBuffer, "-rh %s", "140.118.38.108");
	parser.addDefaultArgument(charBuffer);//if you want to using MobileSim, comment this line

//*********************Simulation or Link to real Robot******************************
	
	ArRobot robot;

	ArRobotConnector robotConnector(&parser, &robot);
	if (!robotConnector.connectRobot())
	{
		if (!parser.checkHelpAndWarnUnparsed())
		{
			ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything");
		}
		else
		{
			ArLog::log(ArLog::Terse, "Error, could not connect to robot.");
			Aria::logOptions();
			Aria::exit(1);
		}
	}
	if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	{    
	Aria::logOptions();
	exit(1);
	}
	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);
	printf("You may press escape to exit\n");
	robot.runAsync(true);
	ArUtil::sleep(1000);//unit: unsigned int ms
	robot.comInt(ArCommands::ENABLE, 1);


//==============================editing your code here=========================

	//robot.setVel(200);			//unit: mm/sec
	//robot.setRotVel(100);			//unit: deg/sec (direction anticlock wise)
	//robot.setVel2(300, 100);		//unit: mm/sec

	//ArUtil::sleep(7000);			//unit: ms
	//robot.stop();
	



//==============================editing your code here=========================

	robot.waitForRunExit();
	Aria::exit(0);

}